/**
 *  @brief SoftI2C
 *  interface to I2C (two-wire) communication.
 */

#include "wirish.h"
#include "gpio.h"
#include "SoftI2C.h"

// Define for PORT/PIN access via gpio_read/write functions
#define SET_PIN_ACCESS()	sda_port = PIN_MAP[sda].gpio_device;	\
							sda_pin  = PIN_MAP[sda].gpio_bit;		\
							scl_port = PIN_MAP[scl].gpio_device;	\
							scl_pin  = PIN_MAP[scl].gpio_bit

#define SCL_HI()			gpio_write_bit(scl_port, scl_pin, 1)
#define SCL_LO()			gpio_write_bit(scl_port, scl_pin, 0)
#define SDA_HI()			gpio_write_bit(sda_port, sda_pin, 1)
#define SDA_LO()			gpio_write_bit(sda_port, sda_pin, 0)
#define SDA_RD()			gpio_read_bit(sda_port, sda_pin)

// #define SET_PIN_ACCESS()
// #define SCL_HI()			digitalWrite(scl, HIGH)
// #define SCL_LO()			digitalWrite(scl, LOW)
// #define SDA_HI()			digitalWrite(sda, HIGH)
// #define SDA_LO()			digitalWrite(sda, LOW)
// #define SDA_RD()			digitalRead(sda)

#define Delay()			SI2C_DELAY(Delay)

#define Delay_SCL_HI()	Delay(); SCL_HI()
#define Delay_SDA_HI()	Delay(); SDA_HI()
#define Delay_SCL_LO()	Delay(); SCL_LO()
#define Delay_SDA_LO()	Delay(); SDA_LO()

void SoftI2C::i2c_start(void)
{
	Delay_SDA_LO();
	Delay_SCL_LO();
}

void SoftI2C::i2c_stop(void)
{
	Delay_SCL_HI();
	Delay_SDA_HI();
}

uint8 SoftI2C::i2c_get_ack(void)
{
	Delay_SCL_LO();
	Delay_SDA_HI();
	Delay_SCL_HI();
	Delay();
	if (!SDA_RD())
	{
		SCL_LO();
		return true;
	}
	else
	{
		SCL_LO();
		return false;
	}
}

void SoftI2C::i2c_send_ack(void)
{
	Delay_SDA_LO();
	Delay_SCL_HI();
	Delay_SCL_LO();
}

void SoftI2C::i2c_send_nack(void)
{
	Delay_SDA_HI();
	Delay_SCL_HI();
	Delay();
	Delay_SCL_LO();
}

uint8 SoftI2C::i2c_shift_in(void)
{
	uint8 data = 0;
	for (uint8 mask = 0x80; mask != 0; mask >>= 1)
	{
		Delay_SCL_HI();
		Delay();
		if (SDA_RD())
			data |= mask;
		SCL_LO();
	}
	return data;
}

void SoftI2C::i2c_shift_out(uint8 val)
{
	for(uint8 mask = 0x80; mask != 0; mask >>= 1)
	{
		if (mask & val)	{ SDA_HI(); }
		else			{ SDA_LO(); }
		Delay_SCL_HI();
		Delay_SCL_LO();
	}
}

SoftI2C::SoftI2C(void)
{
	Delay = 50;
}

/*
 * Joins I2C bus as master on given SDA and SCL pins.
 */
void SoftI2C::begin(uint8 sda, uint8 scl)
{
	SDA = sda;
	SCL = scl;

	SET_PIN_ACCESS();

	pinMode(scl, OUTPUT_OPEN_DRAIN);
	pinMode(sda, OUTPUT_OPEN_DRAIN);

	SCL_HI();
	SDA_HI();

	uint8 retry = 250;
	if (retry-- > 0 && !SDA_RD())
	{
		Delay_SCL_LO();
		Delay_SCL_HI();
	}
}

uint8 SoftI2C::writeReg(uint8 register_address, uint8 data)
{
	return writeReg(SlaveAddress, register_address, data);
}

uint8 SoftI2C::writeReg(uint8 slave_address, uint8 register_address, uint8 data)
{
	uint8 error = SI2C_SUCCESS;
	slave_address <<= 1;

	i2c_start();
	i2c_shift_out(slave_address | SI2C_WRITE);
	if (!i2c_get_ack())
		error = SI2C_ENACKADDR;
	else
	{
		i2c_shift_out(register_address);
		if (!i2c_get_ack())
			error = SI2C_ENACKTRNS;
		else
		{
			i2c_shift_out(data);
			if (!i2c_get_ack())
				error = SI2C_ENACKTRNS;
		}
	}
	Delay_SDA_LO();
	i2c_stop();

	return error;
}

uint8 SoftI2C::readReg(uint8 register_address)
{
	return readReg(SlaveAddress, register_address, &Data);
}

uint8 SoftI2C::readReg(uint8 register_address, uint8 *data)
{
	return readReg(SlaveAddress, register_address, data);
}

uint8 SoftI2C::readReg(uint8 slave_address, uint8 register_address)
{
	return readReg(slave_address, register_address, &Data);
}

uint8 SoftI2C::readReg(uint8 slave_address, uint8 register_address, uint8 *data)
{
	uint8 error = SI2C_SUCCESS;
	slave_address <<= 1;

	i2c_start();
	i2c_shift_out(slave_address | SI2C_WRITE);
	if (!i2c_get_ack())
	{
		Delay_SDA_LO();
		error = SI2C_ENACKADDR;
	}
	else
	{
		i2c_shift_out(register_address);
		if (!i2c_get_ack())
		{
			Delay();
			SDA_LO();
			error = SI2C_ENACKTRNS;
		}
		else
		{
			Delay();
			Delay_SCL_HI();
			Delay();

			i2c_start();
			i2c_shift_out(slave_address | SI2C_READ);
			if (!i2c_get_ack())
			{
				Delay_SDA_LO();
				error = SI2C_ENACKADDR;
			}
			else
			{
				Delay();
				*data = i2c_shift_in();
				i2c_send_nack();
			}
		}
	}
	i2c_stop();
	return error;
}
